//MAP STORE
//This class stores a map as a 2D array
// and publishes it as well as an occupancy grid

#include "mapstore.hpp"
#include "scan_node.hpp"
#include "ros/ros.h"

Mapstore::Mapstore() {
	nodeHandle = NULL;
}

Mapstore::Mapstore(ros::NodeHandle& nh) {
	nodeHandle = &nh;
	mapPub = nh.advertise<nav_msgs::OccupancyGrid>("graph_map", 1);
}

void Mapstore::generateMap(std::vector<Scan_node> nodes) {
	
    Map newMap;
    mapData = newMap;
    mapData.offset = 0;

	Map overlaps;
	
	ROS_INFO("Nodes: %d", nodes.size());
	
	for(int i=0; i<nodes.size(); i++) {
		int node_minx = floorf(nodes[i].getTransform().transform.translation.x / Scan_node::resolution)-nodes[i].offset;
		int node_miny = floorf(nodes[i].getTransform().transform.translation.y / Scan_node::resolution)-nodes[i].offset;
		ROS_INFO("Node %d: (%d, %d) - (%d, %d)", i, node_minx, node_miny, node_minx+nodes[i].render.size(), node_miny+nodes[i].render.size());
		
		//getchar();
		
		//Pre-generate the map
		for(int j=0; j<nodes[i].render.size(); j++) {
            //std::cout << "j: " << j << std::endl;
			overlaps[node_minx][node_miny+j];
			overlaps[node_minx+j][node_miny];
			overlaps[node_minx+nodes[i].render.size()][node_miny+j];
			overlaps[node_minx+j][node_miny+nodes[i].render.size()];
			
			mapData[node_minx][node_miny+j];
			mapData[node_minx+j][node_miny];
			mapData[node_minx+nodes[i].render.size()][node_miny+j];
			mapData[node_minx+j][node_miny+nodes[i].render.size()];
		}
		
		ROS_INFO("Map extended");
		
		
		for(int nx=0; nx<nodes[i].render.size(); nx++) {
            //ROS_INFO("Map column %d", node_minx+nx);
			for(int ny=0; ny<nodes[i].render.size(); ny++) {
                //std::cout << node_miny+ny << " ";
                if(nodes[i].render[nx][ny] >= 0) {
					if(overlaps[node_minx+nx][node_miny+ny] == -1) {
						mapData[node_minx+nx][node_miny+ny] = nodes[i].render[nx][ny];
						overlaps[node_minx+nx][node_miny+ny] = 1;
					}
					else {
						mapData[node_minx+nx][node_miny+ny] += nodes[i].render[nx][ny];
						overlaps[node_minx+nx][node_miny+ny]++;
					}
				}
			}
		}
        //std::cout << std::endl;
		
		/*
		std::cout << "   ";
		for(int x=0; x<nodes[i].render.size(); x++) {
			printf("%03d", x);
		}
		std::cout << std::endl;
		
		for(int y=0; y<nodes[i].render[0].size(); y++) {
			printf("%03d", y);
			for(int x=0; x<nodes[i].render.size(); x++) {
				if(nodes[i].render[x][y] == -1) std::cout << " - ";
				else if(nodes[i].render[x][y] == 0) std::cout << "   ";
				else std::cout << " X ";
			}
			std::cout << std::endl;
		}
		*/
	}
	
	
	ROS_INFO("Patchwork map completed. Normalizing...");
	
	//Map copied, with some overlaps. Normalize values by dividing them with overlap if they aren't -1
	
    for(int x=-mapData.offset; x<(int)mapData.data.size()-mapData.offset; x++) {
        for(int y=-mapData[x].offset; y<(int)mapData[x].data.size()-mapData[x].offset; y++) {
            if(overlaps[x][y] > 0) mapData[x][y] /= overlaps[x][y];
		}
	}
	
	ROS_INFO("Map normalized. Finding origin...");
	
	//Find the origin of the map. Essentially, the minimum X and Y of the bitmap, multiplied by the resolution.
	
    origin.position.x = -mapData.offset * Scan_node::resolution;
	int miny = 0;
	for(int i=0; i<mapData.data.size(); i++) {
        if(-mapData.data[i].offset < miny) miny=-mapData.data[i].offset;
	}
	origin.position.y = miny * Scan_node::resolution;
    origin.position.z = 4;
	
	ROS_INFO("Origin found. Writing to OccupancyGrid...");
	
	mapData.generateMap(map);
	map.info.origin = origin;
	map.info.resolution = Scan_node::resolution;
	map.info.map_load_time = ros::Time::now();
	map.header.stamp = ros::Time::now();
	map.header.frame_id = "map";
	
}

void Mapstore::publishMap() {
	mapPub.publish(map);
}

void Mapstore::tfChangedCallback(Scan_node * node, geometry_msgs::TransformStamped oldTf, geometry_msgs::TransformStamped newTf) {
    if(node->getNumberOfScans() != 0 && nodeVecPtr != NULL) {
        //TODO render only the region of the map that has actually changed
        generateMap(*nodeVecPtr);
    }
}

void Mapstore::dataChangedCallback(Scan_node * node) {
    //TODO render only the changed region
    generateMap(*nodeVecPtr);
}
